// Created by 74354 on 2022/1/2.//
//
#include "boardInit.h"
#include "main.h"
#include "drv8301.h"
#include "Motor.hpp"
#include "adc.h"
#include "stm32f405xx.h"
#include "stm32f4xx_hal_adc.h"

#include "gdtr_define.h"

extern uint16_t Data_RegADC_M0[ADC_RegDATA_SIZE];
extern uint16_t Data_RegADC_M1[ADC_RegDATA_SIZE];
extern uint16_t Data_InjADC_M0[ADC_InjDATA_SIZE];
extern uint16_t Data_InjADC_M1[ADC_InjDATA_SIZE];


Motor_t motor0;
Motor_t motor1;
FOC foc0;
FOC foc1;

Encoder_t enc0;
Encoder_t enc1;

DRV8301_t drv0;
DRV8301_t drv1;


void init_drv8301()
{
    DRV8301_t::DRV8301_Config drv0;
    drv0.gain = M0_DRV8301_I_GAIN;
    drv0.spi = &SPI_DRV8301;
    drv0.ncsPort = M0_NCS_GPIO_Port;
    drv0.ncsPin = M0_NCS_Pin;
    drv0.engatePort = EN_GATE_GPIO_Port;
    drv0.engatePin = EN_GATE_Pin;
    motor0.driver->config(&drv0);
    motor0.driver->setup();

    DRV8301_t::DRV8301_Config drv1;
    drv1.gain = M1_DRV8301_I_GAIN;
    drv1.spi = &SPI_DRV8301;
    drv1.ncsPort = M1_NCS_GPIO_Port;
    drv1.ncsPin = M1_NCS_Pin;
    drv1.engatePort = EN_GATE_GPIO_Port;
    drv1.engatePin = EN_GATE_Pin;
    motor1.driver->config(&drv1);
    motor1.driver->setup();
}

void init_motor()
{
    motor0.encoder = &enc0;
    motor1.encoder = &enc1;
    motor0.driver = &drv0;
    motor1.driver = &drv1;
    motor0.foc_controller = &foc0;
    motor1.foc_controller = &foc1;

    Motor_t::Motor_Config m0{},m1{};
}

void init_foc(){

    FOC::focConfig foc0;
    foc0.timer = M0_driver_timer;
    foc0.ts = TIM_1_8_PERIOD_CLOCKS;
    foc0.udc = U_DC;
    foc0.en_kalman = true;
    foc0.getElecRadian = []() ->float{return motor0.encoder->getRadian() * motor0.pole_pairs;};
    foc0.getMachRadian = []() ->float{return motor0.encoder->getRadian();};
    foc0.get_I_abc = []() ->std::tuple<float,float,float>{
        motor0.compute_Current();
        std::tuple<float,float,float> current(motor0.iABC_ANO[0],motor0.iABC_ANO[1],motor0.iABC_ANO[2]);
        return current;
    };

    FOC::focConfig foc1;
    foc1.timer = M1_driver_timer;
    foc1.ts = TIM_1_8_PERIOD_CLOCKS;
    foc1.udc = U_DC;
    foc1.en_kalman = true;
    foc1.getElecRadian = [](){return motor1.encoder->getRadian() * motor0.pole_pairs;};
    foc1.getMachRadian = [](){return motor1.encoder->getRadian();};
    foc1.get_I_abc = []() ->std::tuple<float,float,float>{
        motor1.compute_Current();
        std::tuple<float,float,float> current(motor1.iABC_ANO[0],motor1.iABC_ANO[1],motor1.iABC_ANO[2]);
        return current;
    };

    motor0.foc_controller->config(&foc0);
    motor1.foc_controller->config(&foc1);
}

inline uint16_t get_M0_count()  {return __HAL_TIM_GET_COUNTER(&(motor0.encoder->encoder_timer));}
inline uint16_t get_M1_count()  {return __HAL_TIM_GET_COUNTER(&(motor1.encoder->encoder_timer));}

void init_encoder()
{
    Encoder_t::ENC_Config en0;
    en0.type = M0_enc_type;
    en0.timer = M0_enc_timer;
    en0._cpr = M0_enc_cpr;

    Encoder_t::ENC_Config en1;
    en1.type = M1_enc_type;
    en1.timer = M1_enc_timer;
    en1._cpr = M1_enc_cpr;

    motor0.encoder->config(&en0);
    motor1.encoder->config(&en1);

    motor0.encoder->getCount = get_M0_count;
    motor1.encoder->getCount = get_M1_count;

    __HAL_TIM_CLEAR_IT(&htim3,TIM_IT_UPDATE);
    HAL_TIM_Encoder_Start(&M0_enc_timer, TIM_CHANNEL_ALL);
    __HAL_TIM_ENABLE_IT(&htim3,TIM_IT_UPDATE);
    __HAL_TIM_CLEAR_IT(&htim4,TIM_IT_UPDATE);
    HAL_TIM_Encoder_Start(&M1_enc_timer, TIM_CHANNEL_ALL);
    __HAL_TIM_ENABLE_IT(&htim4,TIM_IT_UPDATE);

}

void init_adc_driverTimer()
{
    HAL_ADC_Start(&ADC_M0);
    HAL_ADC_Start(&ADC_M1);

    HAL_ADCEx_InjectedStart(&ADC_M0);
    HAL_ADCEx_InjectedStart(&ADC_M1);

    __HAL_ADC_ENABLE_IT(&ADC_M0,ADC_IT_JEOC);
    __HAL_ADC_ENABLE_IT(&ADC_M1,ADC_IT_JEOC);

    /* 使能下面的语句会开启HAL_ADC_ConvCpltCallback 即EOC事件*/
//	__HAL_ADC_ENABLE_IT(&ADC_M0,ADC_IT_EOC);
//	__HAL_ADC_ENABLE_IT(&ADC_M1,ADC_IT_EOC);

    HAL_ADC_Start_DMA(&ADC_M0,(uint32_t*)Data_RegADC_M0,ADC_RegDATA_SIZE);
    HAL_ADC_Start_DMA(&ADC_M1,(uint32_t*)Data_RegADC_M1,ADC_RegDATA_SIZE);

    HAL_TIM_PWM_Start(&M0_driver_timer,TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&M0_driver_timer,TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&M0_driver_timer,TIM_CHANNEL_3);
    HAL_TIM_PWM_Start(&M0_driver_timer,TIM_CHANNEL_4);
    HAL_TIMEx_PWMN_Start(&M0_driver_timer,TIM_CHANNEL_1);
    HAL_TIMEx_PWMN_Start(&M0_driver_timer,TIM_CHANNEL_2);
    HAL_TIMEx_PWMN_Start(&M0_driver_timer,TIM_CHANNEL_3);
    HAL_TIM_PWM_Start(&M1_driver_timer,TIM_CHANNEL_4);
}


/* EOC事件回调函数 */
void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc)
{
    if(hadc == &ADC_M0){}
    if(hadc == &ADC_M1){}
}

static uint32_t timeCount = 0;
float getTimeNow()
{
    return timeCount/500 + __HAL_TIM_GET_COUNTER(&htim5)/1000000.0f;
}

uint32_t getTimeNowInt()
{
    return timeCount;
}
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef* htim){
    if(htim == (&htim5))timeCount++;

    if(htim == &htim3)
    {
        if(!__HAL_TIM_IS_TIM_COUNTING_DOWN(&motor0.encoder->encoder_timer)) motor0.encoder->setCircleCount(motor0.encoder->getCircleCount() + 1);
        else if(__HAL_TIM_IS_TIM_COUNTING_DOWN(&motor0.encoder->encoder_timer)) motor0.encoder->setCircleCount(motor0.encoder->getCircleCount() - 1);

    }
    if(htim == &(htim4))
    {
        if(!__HAL_TIM_IS_TIM_COUNTING_DOWN(&motor1.encoder->encoder_timer))motor1.encoder->setCircleCount(motor1.encoder->getCircleCount() + 1);
        else if(__HAL_TIM_IS_TIM_COUNTING_DOWN(&motor1.encoder->encoder_timer))motor1.encoder->setCircleCount(motor1.encoder->getCircleCount() - 1);
    }
}




/* 注入通道 EOC事件 回调函数 */
void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef* hadc)
{
    int temp0,temp1;

    if(hadc == &ADC_M0)
    {
        temp0 = motor0.iABC_dig[1] = hadc->Instance->JDR1 - motor0.i_phase_offset[1] - 2048;
        temp1 = motor0.iABC_dig[2] = hadc->Instance->JDR2 - motor0.i_phase_offset[2] - 2048;
        motor0.iABC_dig[0] = - (temp0 + temp1);
    }
    if(hadc == &ADC_M1)
    {
        temp0 = motor1.iABC_dig[1] = hadc->Instance->JDR1 - motor1.i_phase_offset[1] - 2048;
        temp1 = motor1.iABC_dig[2] = hadc->Instance->JDR2 - motor1.i_phase_offset[2] - 2048;
        motor1.iABC_dig[0] = - (temp0 + temp1);
    }
}

/* USER CODE BEGIN 2 */
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
    /* DRV8303错误中断 */
    if(GPIO_Pin == NFAULT_Pin){
        motor0.driver->setup();
        motor1.driver->setup();
        USER_LOG(USER_LOG_ERR,"ERROR:Drv8301");
    }

    /* 释放电机并等待 */
    else if(GPIO_Pin == GPIO_5_Pin){
        motor0.foc_controller->release();
        motor1.foc_controller->release();
        while (true){
            USER_LOG(USER_LOG_INFO,"Waiting !!!!!");
        }
    }

    /* 复位信号 */
    else if(GPIO_Pin == GPIO_8_Pin){
        USER_LOG(USER_LOG_EXIT,"Restarted !!!!!");
        osDelay(500);
        if(HAL_GPIO_ReadPin(GPIO_8_GPIO_Port,GPIO_8_Pin))HAL_NVIC_SystemReset();
    }

    /* 编码器Z相中断 */
    else if(GPIO_Pin == M1_ENC_Z_Pin)USER_LOG(USER_LOG_INFO,"Waiting !!!");//motor1.encoder->zPhaseAround = true;

    else if(GPIO_Pin == M0_ENC_Z_Pin)motor0.encoder->zPhaseAround = true;
}


